package simulator.implementation.agent;
import javax.vecmath.Vector3d;

import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simulator.meta.agent.ControlledAgent;
public class SonarRobot extends ControlledAgent{
	//Constants:
	private final byte NUMBER_OF_ULTRASONIC_SENSORS = 10;
	//Attributes:
	private RangeSensorBelt sonars;
	//Constructor: 
	public SonarRobot(Vector3d position, String name) {
		super(position, name);
		this.step = 0;
		this.turn = 0;
		this.sonars = RobotFactory.addSonarBeltSensor(this, this.NUMBER_OF_ULTRASONIC_SENSORS);
	}
	//Methods:
	public void initBehavior() {}

	public void performBehavior() {
		this.setTranslationalVelocity(this.step);
		this.setRotationalVelocity((Math.PI/180)*this.turn);
		//Cada 10 tiempos del ambiente, se obtienen mediciones:
		if((this.getCounter()%10)==0){
			double measure; boolean hit;
			System.out.println("=========================");
			//Por cada sensor...
			for(int i = 0;i<this.sonars.getNumSensors();i++){
				/*...obtener su medición.
				 * Si el sensor no detecta objetos, la medición
				 * será "infinito".
				*/
				measure = this.sonars.getMeasurement(i);
				//Obtener si detecta algo.
				hit = this.sonars.hasHit(i);
				if(hit){
					//Si detecta algo, entonces lo imprimimos.
					System.out.println("US "+i+": "+measure+", "+hit);
				}
			}
		}
	}
}
